/*
 * SensorPoller.cpp
 *
 *  Created on: 4 dec. 2012
 *      Author: Admin
 */

#include "Robot.hpp"
#include "Sensor.hpp"

//Events
#include "CollisionEvent.hpp"
#include "CrossRoadsEvent.hpp"
#include "ProximityEvent.hpp"

//Sensor(poller)s
#include "SensorPoller.hpp"
#include "CollisionSensor.hpp"
#include "ColorLaneSensor.hpp"
#include "CrossRoadsSensor.hpp"
#include "DistanceSensor.hpp"

SensorPoller::SensorPoller() : sensorCount(0)
{
	//Init sensorpoller sensors
	addSensor(new CollisionSensor(BACK));
	addSensor(new CollisionSensor(FRONT));
	addSensor(new DistanceSensor());
	addSensor(new CrossRoadsSensor());
}

SensorPoller::~SensorPoller()
{
	for (short i = 0; i < sensorCount; i++)
		delete sensors[i];
}

void SensorPoller::poll()
{
	for (short i = 0; i < sensorCount; i++)
	{
		//Note!! (*it) points towards pointer to sensor!!!

		//Get sensor pointer
		Sensor* sensorPointer = sensors[i];

		//Get last and new value of this sensor
		bool 	lastValue = lastValues[i],
				newValue = sensorPointer->poll();

		//Handle sensor
		switch (sensorPointer->getSensorType())
		{
			case Sensor::COLLISION:
			{
				Side side = ((CollisionSensor*) sensorPointer)->getSide();
				pollCollisionSensor(side ,lastValue, newValue);
				break;
			}
			case Sensor::CROSSROADS:
			{
				pollCrossRoadsSensor(lastValue, newValue);
				break;
			}
			case Sensor::DISTANCE:
			{
				pollDistanceSensor(lastValue, newValue);
				break;
			}
			default:
				break;
		}

		//Save the new value as last value
		lastValues[i] = newValue;
	}
	//Poll colorlanesensors
	LaneSensorPoller::getInstance().pollLaneSensors();
}

void SensorPoller::addSensor(Sensor* sensor)
{
	if (sensorCount == 10) //Max 10 sensors allowed
		return;
	lastValues[sensorCount] = sensor->poll();
	sensors[sensorCount++] = sensor;
}

void SensorPoller::pollCollisionSensor(Side side, bool lastValue, bool newValue)
{
	if (newValue == 1) //(existing) collision occurred
	{
		CollisionEvent event(side);
		Robot::getInstance().handleEvent(event);
	}
}

void SensorPoller::pollCrossRoadsSensor(bool lastValue, bool newValue)
{
	if (lastValue == 0 && newValue == 1) //just entered crossroads
	{
		CrossRoadsEvent event(1);
		Robot::getInstance().handleEvent(event);
	}
	else if (lastValue == 1 && newValue == 0) //just left crossroads
	{
		CrossRoadsEvent event(0);
		Robot::getInstance().handleEvent(event);
	}
}

void SensorPoller::pollDistanceSensor(bool lastValue, bool newValue)
{
	if (newValue == 1) //(existing) collision occurred
	{
		ProximityEvent event;
		Robot::getInstance().handleEvent(event);
	}
}

